/**
 * @file imulib.h
 * @author Yifan Xiao (1993996310@qq.com)
 * @brief 6 轴 IMU 姿态解算库
 * @version 0.1
 * @date 2023-03-09
 *
 * @copyright Copyright (c) 2023
 *
 */

#ifndef __IMULIB_H__
#define __IMULIB_H__

#include "main.h"
#include "SEEKFREE_MPU6050.h"
#include "SEEKFREE_IIC.h"
#include "math.h"


/**
 * @brief 欧拉角结构体
 *
 */
typedef struct
{
    float roll;  // 横滚角    
    float pitch; // 俯仰角    
    float yaw;   // 偏航角
} EulerAngle;

/**
 * @brief IMU数据结构体
 *
 */
typedef struct
{
    float acc[3]; // 加速度计数据, 单位: m/s^2, 分别为 x, y, z 轴
    float gyr[3]; // 陀螺仪数据, 单位: rad/s, 分别为 x, y, z 轴
} IMUData;


/**
 * @brief IMU 姿态解算上下文结构体
 *
 */
struct IMUContextStruct
{
    float q0; // 四元数 q0
    float q1; // 四元数 q1
    float q2; // 四元数 q2
    float q3; // 四元数 q3

    float exInt; // 误差积分
    float eyInt; // 误差积分
    float ezInt; // 误差积分

    float Kp; // 比例增益
    float Ki; // 积分增益
};

typedef struct IMUContextStruct IMUContext;


extern IMUContext imu_ctx;
extern IMUData imu_input;
extern EulerAngle imu_euler;

/**
 * @brief IMU 解算初始化
 * 
 * @param imu_content 解算上下文
 * @param imu_data  IMU 数据结构体(输入)
 * @param euler_angle 欧拉角结构体(输出)
 */
void IMU_Init(IMUContext *imu_context, IMUData *imu_data, EulerAngle *euler_angle);

/**
 * @brief IMU 解算
 * 
 * @param imu_content 解算上下文
 * @param imu_data IMU 数据结构体(输入)
 * @param euler_angle 欧拉角结构体(输出)
 * @param dt 周期，单位: s
 */


void IMU_Handler(uint16_t cycle_tick);   //IMU 主要处理函数


#endif
